/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/opInclude/operation_turn_around_once.h
 * @Description  : 该文件定义了OperationTurnAroundOnce类，该类用于实现AGV在转向逻辑。 
 *                 包含多个阶段的操作，如准备、旋转、直线行驶等，并通过ROS接口与外部系统交互。 
 *                 该类还处理与载货状态、相机数据获取和运动控制相关的逻辑。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:38:36
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "operation.h"
#include "operation_peripheral.h"
#include "trajectory.h"

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "OperationTurnAroundOnce".
class OperationTurnAroundOnce : public Operation
{
public:
    explicit OperationTurnAroundOnce(const agv_msgs::msg::Action& action);

    // The destructor
    virtual ~OperationTurnAroundOnce();

    // Initialize the related data
    virtual bool Init() override;

    // Ready to start new op
    virtual bool ReadyStart() override;

    // Defines how to start the operation
    virtual bool StartAction() override;

    // Defines how to end the operation
    virtual bool EndAction() override;

    // Defines how to do the operation
    virtual bool Work() override;

    // 循环函数
    bool CycleProc();

public:
    // for perial call back function
    void FeedbackCb(GoalHandlePeripheral::SharedPtr,
      const std::shared_ptr<const Peripheral::Feedback> feedback);

    void DoneCb(const GoalHandlePeripheral::WrappedResult & result);

    void ActiveCb(GoalHandlePeripheral::SharedPtr goal_handle);

private:
    enum CAMERA_TYPE {             // 当前使用的二维码读头的类型（下读头或上读头）{
        CAMERA_CHECK_GROUND,       // 检测地面二维码
        CAMERA_CHECK_RACK          // 检测货架二维码
    };

    enum TURN_PHASE {
        PHASE_READY_START = -1,    // 预开始准备阶段
        PHASE_START = 0,           // 开始阶段
        PHASE_FIRST_SPIN = 1,      // 开始第一次旋转
        PHASE_LINE = 2,            // 走一条直线
        PHASE_SECOND_SPIN = 3,     // 开始第二次旋转
        PHASE_THIRD_SPIN = 4,      // 开始第三次旋转,旋转到目标设定角度
        PHASE_CHECK_X_Y_ANGLE = 5, // 任务完成进入等待阶段或检测是否到位
        PHASE_UPDATE_POS = 6,      // 完成等待退出
        PHASE_LOAD = 7,            // 进行取货举升阶段
        PHASE_FINISH = 8,          // 完成等待退出
        PHASE_CALC_TARGET_ANGLE = 9
    };

    enum SPIN_PHASE {
        SPIN_READY_START = 0,      // 矫正角度：等待自旋开始
        SPIN_START_OK = 1          // 矫正角度：自旋开始
    };

    enum LOAD_PHASE {
        LOAD_INIT = 0,
        LOAD_READY_START = 1,
        LOAD_START = 2,
        LOAD_WORK = 3,
        LOAD_READY_END = 4,
        LOAD_END = 5,
        LOAD_ERROR = 6
    };

    enum CALC_DATA_RESULT {
        RET_MAX_TIMES_ERROR = -4,      // 最大矫正次数仍失败错误
        RET_LOAD_STATUS_ERROR = -3,    // 货物状态异常,已经处于载货
        RET_GET_LOAD_DATA_FAILED = -2, // 获取货物信息超时,topic无数据
        RET_GET_DATA_FAILED = -1,      // 获取二维码数据超时
        RET_X_Y_ANGLE_TARGET = 0,      // x  y  angle 是否符合0 0 0
        RET_X_Y_TARGET = 1,            // x y  达到目标
        RET_ANGLE_TARGET = 2,          // 角度达到目标
        RET_NEXT_STAGE = 3,            // 正常进入下一阶段
        RET_RESTART = 4                // 再次进入矫正初始化循环阶段
    };

    struct CAMERA_POS {
        float x;
        float y;
        float theta;
        CAMERA_POS() : x(0.0f), y(0.0f), theta(0.0f) {}
    };

    struct MOVE_DATA {
        float start_spin_angle;
        float frist_line;
        float start_spin_back_angle;
        float end_spin_angle;
        MOVE_DATA() 
            : start_spin_angle(0.0f), 
            frist_line(0.0f), 
            start_spin_back_angle(0.0f),
            end_spin_angle(0.0f) {}
    };

    bool HandleLoadAcitonCycle();

    // Initialize the load related data
    bool LoadInit();

    // Ready to start new load op
    bool LoadReadyStart();

    // Defines how to start the load
    bool LoadStart();

    // Defines how to end the load
    bool LoadEnd();

    // Defines how to do the load
    bool LoadWork();
private:
    // ros相关函数和变量

    // 预准备阶段
    // void StartPreTune();

    //  初始化ros相关变量
    bool InitRosValue();

    // 获取载货状态
    bool GetLoadStatus(bool &is_load);

    // 获取下读头数据
    bool GetGroundCameraData(float &pos_x, float &pos_y, float &angle);

    // 获取上读头数据
    bool GetRackCameraData(float &pos_x, float &pos_y, float &angle);

    // 获取里程计的指定字段内容
    bool GetOdomData(float &vx, float &vz, float &theta);

    // 判断数据的时效性
    // bool IsTimeOut(const rclcpp::Time &data_time, const rclcpp::Time &cur_time, const double &max_time_ms);

    // 获取实际反馈的速度
    bool GetRealVel(float &vx, float &vz);

    // 提取到的原始二维码数据到普通pos格式
    bool RawDataToPos(const geometry_msgs::msg::PoseStamped &camera_data_pos, Posture &output_data_pos);

    // 相对零点偏移
    bool ToRelativePos(const geometry_msgs::msg::PoseStamped &camera_data_pos, const Posture &input_relative_pos, Posture &output_data_pos);

    // 获取当前照相机的原始数据
    geometry_msgs::msg::PoseStamped GetCurCameraRawData();

private:
    // 设置矫正阶段
    void SetPhase(int phase);

    // 初始化变量值或读取参数服务器
    void SetDefaultValue();

    // 复位运行状态变量值
    void ResetStatus();

    // 预先就绪阶段，查询相对零位
    int ReadyStartTune();

    // 开始调整阶段
    int StartTune();

    // 设置相对参考零位(QR旋转方向)
    int SetRelativePos();

    // 移动清除角度误差
    bool SpinMove(float angle);

    // 控制车体走一段直线轨迹
    bool StraightMove(float distance);

    // 将车体停止
    void Stop();

    // 检测照相机是否数据丢失或超时
    void CheckCameraLostData();

    // 清除数据丢失报警标志
    void ClearCameraLostFlag();

    // 暂时不使用，当数据来源多个的时候，可以启用此接口
    bool ReadPos(float &pos_x, float &pos_y, float &angle);

    // 读取robot相对于二维码的位置偏差
    bool ReadCameraPos(int type, float &pos_x, float &pos_y, float &angle);

    // 读取里程记的当前角度
    bool GetRelAngle(float &angle);

    // PI/2的整数倍数;四舍五入
    // fTheta:弧度
    int MatchToMultipleOfPi_2(float theta);

    // PI/2 的余数弧度
    // fTheta:弧度 返回值 绝对值
    float RemainderDivPi_2(float theta);

    // 最大矫正失败错误
    void SetMaxTimesError(bool occur);

private:
    // 根据当前偏差信息计算当前循环的运动数据
    int CalcTurnData();

    // 是否到达目标位置
    bool IsTurnTarget(float x, float y, float angle);

    // 是否平移误差可以忽略
    bool IsXYTurnTarget(float x, float y);

    // bool IsSpinAngleTarget(float fSPinTargetAngle, float fCurAngle);

    // 设置可移动的数据值
    void SetMoveValue(CAMERA_POS pos);

    // 第一次旋转
    bool FirstSpin();

    // 走直线
    bool MoveLine();

    // 第二次旋转到0度
    bool SecondSpin();

    // 第三次旋转到目标角度
    bool ThirdSpin();

    // 空闲阶段；是否需要再次矫正
    int CheckXYAngle();

    // 更新位姿态
    bool UpdatePosProc();

    // 清除自旋标志
    void ClearSpinFlag();

    //  
    int CalcTargetAngle();

private:
    // 到达目标位置的阈值
    float limit_x_;            // X方向（前后）矫正的到达目标的门限值 （m）
    float limit_y_;            // Y方向（左右）矫正的到达目标的门限值 (m)
    float limit_theta_;        // 角度方向矫正的到达目标的门限值（弧度）
    float max_spin_vel_;       // 最大自旋速度 rad/s
    float min_spin_vel_;       // 最小自旋速度 rad/s
    float velocity_;           // 走直线速度 m/s
    int max_adjust_times_;     // 最大允许调整次数
    int current_adjust_times_; // 记录当前调整的次数
    int division_k_;           // 平行于坐标轴的分频系数值(for example;m_fLimitX/k)
private:
    float current_line_progress_; // 走斜线时，当前短直线的进度
    int spin_phase_;           // 清除Y方向误差时，旋转步骤
private:
    //
    int phase_;                // 当前正在进行矫正的步骤
    int camera_lost_count_;    // 照相机丢失的计数
    TIME_TYPE last_time_;      // 记录上一次循环的执行的时间点
    float last_real_angle_;    // 清除角度偏差时，自旋时，记录里程计上一次的相对角度值
    float micro_angle_diff_;   // 清除角度偏差时，自旋时，累加微小的角度(积分角度)
    float turned_angle_;       // 清除角度偏差时，自旋时，累计已经旋转过的角度值 带有正负号
    float cycle_;              // 当前实际循环执行的时间周期
    bool rack_camera_lost_;    // 上读头相机是否报数据丢失错误
    bool ground_camera_lost;   // 下读头相机是否报数据丢失错误
    int  current_camera_type_; // 当前使用的二维码读头的类型（下读头或上读头）
    Posture relative_zero_pos_;// 相对零点偏差

    MOVE_DATA  move_data_;     // 当前循环的移动数据
    CAMERA_POS camera_pos_;    // 当前正在使用的照相机的位置
    LOAD_PHASE load_phase_;    // 取货阶段

    bool filter_angle_;  // 使用1.5倍角度到位门限，防到位后下周期再跳变
    float filter_angle_k_;  // 角度阈值倍数
};